/**
 * @file   jps_normal.cpp
 * @brief  jump point search algorithm
 * @encode UTF-8
 * @log    date        author      notes
 *         2021.09.22  Jimmy       first version
 */
#include "jps_normal.h"

const uint8_t STRAIGHT_COST = 10;   /* 直线移动的成本 */
const uint8_t SLASH_COST    = 14;   /* 对焦移动的成本 */


/**
 * @brief  Construct a new Jps Normal:: Jps Normal object
 * @author Jimmy 
 * @date   2021.09.22
 */
JpsNormal::JpsNormal()
{

}

/**
 * @brief  Destroy the Jps Normal:: Jps Normal object
 * @author Jimmy 
 * @date   2021.09.22
 */
JpsNormal::~JpsNormal()
{

}

/**
 * @brief  计算节点的G
 * @param  nPoint           
 * @param  prePoint       
 * @return uint8_t 
 * @author Jimmy 
 * @date   2021.09.22
 * @note   prePoint  ----->   nPoint
 */
uint16_t JpsNormal::calcG(Point nPoint, Point prePoint)
{
    uint16_t dx = std::abs(nPoint.x - prePoint.x);
    uint16_t dy = std::abs(nPoint.y - prePoint.y);
    
    uint16_t cost    = (dx + dy == 1) ? (STRAIGHT_COST) : (SLASH_COST);
    /* close list的元素第四位放的是g值 */
    uint8_t preCost = mClosedList->getIndex(prePoint);

    return (cost + preCost);
}

/**
 * @brief  计算节点的G
 * @param  nPoint           
 * @param  destPoint       
 * @return uint8_t 
 * @author Jimmy 
 * @date   2021.09.22
 * @note   nPoint  ----->  destPoint
 */
uint16_t JpsNormal::calcH(Point nPoint, Point destPoint)
{
    /* 对角线距离 */
    uint16_t dx = abs(nPoint.x - destPoint.x);
    uint16_t dy = abs(nPoint.y - destPoint.y);
    uint16_t hVal = STRAIGHT_COST * (dx + dy) - 2 * STRAIGHT_COST * std::min(dx, dy) + SLASH_COST * std::min(dx, dy);

    return hVal;
}


/* End of file */
